{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys ###kfglobal1kf（下のセルも）\n",
    "sys.path.append('../scripts/')\n",
    "from kf import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "class GlobalKf(KalmanFilter):\n",
    "    def __init__(self, envmap, motion_noise_stds={\"nn\":0.19, \"no\":0.001, \"on\":0.13, \"oo\":0.2},\\\n",
    "                 distance_dev_rate=0.14, direction_dev=0.05): #姿勢の引数を消す\n",
    "        super().__init__(envmap, np.array([0, 0, 0]).T, motion_noise_stds, distance_dev_rate, direction_dev) #初期姿勢は適当に\n",
    "        self.belief.cov = np.diag([1e+4, 1e+4, 1e+4])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def trial(animation): ###kfglobal1test\n",
    "    time_interval = 0.1\n",
    "    world = World(30, time_interval, debug=not animation) \n",
    "\n",
    "    ## 地図を生成して3つランドマークを追加 ##\n",
    "    m = Map()\n",
    "    m.append_landmark(Landmark(-4,2))\n",
    "    m.append_landmark(Landmark(2,-3))\n",
    "    m.append_landmark(Landmark(3,3))\n",
    "    world.append(m)\n",
    "\n",
    "    ## ロボットを作る ##\n",
    "    init_pose = np.array([np.random.uniform(-5.0, 5.0), np.random.uniform(-5.0, 5.0), np.random.uniform(-math.pi, math.pi)]).T\n",
    "    kf = GlobalKf(m)\n",
    "    a = KfAgent(time_interval, 0.2, 10.0/180*math.pi, kf)\n",
    "    r = Robot(init_pose, sensor=Camera(m), agent=a, color=\"red\")\n",
    "    world.append(r)\n",
    "\n",
    "    world.draw()\n",
    "    \n",
    "    return (r.pose, a.kf.belief.mean)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "ename": "TypeError",
     "evalue": "__init__() missing 1 required positional argument: 'motion_noise_stds'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mTypeError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-4-c1bddb59977f>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      2\u001b[0m     \u001b[0mok\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;36m0\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      3\u001b[0m     \u001b[0;32mfor\u001b[0m \u001b[0mi\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mrange\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m1000\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 4\u001b[0;31m         \u001b[0mactual\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mestm\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mtrial\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;32mFalse\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m      5\u001b[0m         \u001b[0mdiff\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mmath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msqrt\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mactual\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0mestm\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m**\u001b[0m\u001b[0;36m2\u001b[0m \u001b[0;34m+\u001b[0m \u001b[0;34m(\u001b[0m\u001b[0mactual\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0mestm\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m**\u001b[0m\u001b[0;36m2\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      6\u001b[0m         \u001b[0mprint\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mi\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m\"真値:\"\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mactual\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m\"推定値\"\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mestm\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m\"誤差:\"\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mdiff\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m<ipython-input-3-fd3a220ce12c>\u001b[0m in \u001b[0;36mtrial\u001b[0;34m(animation)\u001b[0m\n\u001b[1;32m     12\u001b[0m     \u001b[0;31m## ロボットを作る ##\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     13\u001b[0m     \u001b[0minit_pose\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mnp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0marray\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mnp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mrandom\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0muniform\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;36m5.0\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m5.0\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mnp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mrandom\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0muniform\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;36m5.0\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m5.0\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mnp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mrandom\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0muniform\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0mmath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpi\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mmath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpi\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mT\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 14\u001b[0;31m     \u001b[0mkf\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mGlobalKf\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mm\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     15\u001b[0m     \u001b[0ma\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mKfAgent\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mtime_interval\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m0.2\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m10.0\u001b[0m\u001b[0;34m/\u001b[0m\u001b[0;36m180\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0mmath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpi\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mkf\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     16\u001b[0m     \u001b[0mr\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mRobot\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0minit_pose\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0msensor\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mCamera\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mm\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0magent\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0ma\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mcolor\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;34m\"red\"\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mTypeError\u001b[0m: __init__() missing 1 required positional argument: 'motion_noise_stds'"
     ]
    }
   ],
   "source": [
    "if __name__ == '__main__': ###mclglobal1exec\n",
    "    ok = 0\n",
    "    for i in range(1000):\n",
    "        actual, estm = trial(False)\n",
    "        diff = math.sqrt((actual[0]-estm[0])**2 + (actual[1]-estm[1])**2)\n",
    "        print(i, \"真値:\", actual, \"推定値\", estm, \"誤差:\", diff)\n",
    "        if diff <= 1.0:\n",
    "            ok += 1\n",
    "\n",
    "    ok"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
